#define NUM_OF_POS 400
#define NUM_OF_NEG 400
#define NUM_OF_EXAMPLES NUM_OF_POS+NUM_OF_NEG
#define SUBWINDOW_SIZE 14 * 35

#include "time.h"
#include "math.h"
#include "mex.h"

int length_find(double *array, double threshold, int size, int flag) {
    int i, res = 0;
    switch (flag) {
        case 1:
            for (i = 0; i < size; i++)
                if (array[i] > threshold) res++;
			break;
        case 0:
            for (i = 0; i < size; i++)
                if (array[i] < threshold) res++;
			break;
        default:
            printf("ERROR: Incorrect length_find flag.\n");
            return -1;   
    }
    return res;
}

void weak_classifier(double mu_neg, double sig_neg, double mu_pos, double sig_pos,
                       double *feature, double *weights,
             /* out */ double *err, double *threshold, int *parity) {
    double threshold_1, threshold_2, a1, a2, a, b, c;
    double negative[NUM_OF_NEG], positive[NUM_OF_POS];
        
    if (sig_neg == sig_pos) {
        threshold_1 = (mu_neg + mu_pos) / 2;
        threshold_2 = threshold_1;
    }
    else {
        a1 = sig_neg * sig_pos * log(abs(sig_neg));
        a2 = sig_neg * sig_pos * log(abs(sig_pos));
        
        a = sig_pos - sig_neg;
        b = sig_pos * mu_neg - sig_neg * mu_pos;
        c = sig_pos * pow(mu_neg, 2) - sig_neg * pow(mu_pos, 2) + a1 - a2;
        
        threshold_1 = (b + sqrt(pow(b, 2) - a * c)) / a;
        threshold_2 = (b - sqrt(pow(b, 2) - a * c)) / a;
    }
    
    memcpy(negative, feature, sizeof(double) * NUM_OF_NEG);
    memcpy(positive, feature + NUM_OF_NEG, sizeof(double) * NUM_OF_POS);
        
	if (mu_neg < mu_pos) {
        *threshold = threshold_1 < threshold_2 ? threshold_1 : threshold_2;
        *parity = 1;
        *err = (double)(length_find(negative, *threshold, NUM_OF_NEG, 1) + length_find(positive, *threshold, NUM_OF_POS, 0)) / NUM_OF_EXAMPLES;
    }
    else {
        *threshold = threshold_1 > threshold_2 ? threshold_1 : threshold_2;
        *parity = -1;
        *err = (double)(length_find(negative, *threshold, NUM_OF_NEG, 0) + length_find(positive, *threshold, NUM_OF_POS, 1)) / NUM_OF_EXAMPLES;
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    
    /* Declaration and initialization */
    /* Input arguments */
    double *m_negative = mxGetPr(prhs[0]);
    double *m_positive = mxGetPr(prhs[1]);
    double *sigma_negative = mxGetPr(prhs[2]);
    double *sigma_positive = mxGetPr(prhs[3]);
    double *weights = mxGetPr(prhs[4]);
    double *num_of_features = mxGetPr(prhs[5]);
    /* Output arguments */
    double *err, *threshold, *parity;
    /* Feature and gvector binaries */
    FILE *fid_feat = fopen("f_out.bin", "rb");
	FILE *fid_gvec = fopen("g_out.bin", "rb");
    /* Counters */
    int i, j, k;
    /* Temp variables inside the lopp */
    int features[NUM_OF_EXAMPLES], gvectors[SUBWINDOW_SIZE], cur_parity;
    double cur_feat[NUM_OF_EXAMPLES], cur_gvec[SUBWINDOW_SIZE], cur_err, cur_threshold;
    double mu_negative = 0, mu_positive = 0, sig_negative = 0, sig_positive = 0, interm[SUBWINDOW_SIZE];
    /* Time variables */
    //time_t tic, toc;
        
    /* Allocate memory and assign output pointer */
    plhs[0] = mxCreateDoubleMatrix(1, *num_of_features, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(1, *num_of_features, mxREAL);
    plhs[2] = mxCreateDoubleMatrix(1, *num_of_features, mxREAL);
    err = mxGetPr(plhs[0]);
    threshold = mxGetPr(plhs[1]);
    parity = mxGetPr(plhs[2]);
        
    for (i = 0; i < *num_of_features; i++) {
        
        fread(features, sizeof(int), NUM_OF_EXAMPLES, fid_feat);
        fread(gvectors, sizeof(int), SUBWINDOW_SIZE, fid_gvec);
        
        for (j = 0; j < NUM_OF_EXAMPLES; j++)
            cur_feat[j] = (double)features[j];
        for (j = 0; j < SUBWINDOW_SIZE; j++)
            cur_gvec[j] = (double)gvectors[j];
        
        free(features); free(gvectors);
		        
        /* mu_negative = m_negative'*current_gvector */
		for (j = 0; j < SUBWINDOW_SIZE; j++) {
            if (cur_gvec[j] == 0) continue;
//             if (cur_gvec[j] == 1) mu_negative += m_negative[j];
//             else 
            mu_negative += (m_negative[j] * cur_gvec[j]);
        }
		                
        /* mu_positive = m_positive'*current_gvector */
		for (j = 0; j < SUBWINDOW_SIZE; j++) {
            if (cur_gvec[j] == 0) continue;
//             if (cur_gvec[j] == 1) mu_positive += m_positive[j];
//             else 
            mu_positive += (m_positive[j] * cur_gvec[j]);
        }
                        
        /* sig_negative = current_gvector'*sigma_negative*current_gvector */
		for (k = 0; k < SUBWINDOW_SIZE; k++) {
			interm[k] = 0;
            for (j = 0; j < SUBWINDOW_SIZE; j++) {
				if (cur_gvec[j] == 0)  continue;
//                 if (cur_gvec[j] == 1) interm[k] += sigma_negative[j * SUBWINDOW_SIZE + k];
//                 else 
                interm[k] += (cur_gvec[j] * sigma_negative[j * SUBWINDOW_SIZE + k]);
			}
		}        
        for (j = 0; j < SUBWINDOW_SIZE; j++) {
            if (cur_gvec[j] == 0) continue;
//             if (cur_gvec[j] == 1) sig_negative += interm[j];
//             else 
            sig_negative += (interm[j] * cur_gvec[j]);
        }
                		       
        /* sig_positive = current_gvector'*sigma_positive*current_gvector */
		for (k = 0; k < SUBWINDOW_SIZE; k++) {
			interm[k] = 0;
            for (j = 0; j < SUBWINDOW_SIZE; j++) {
				if (cur_gvec[j] == 0)  continue;
//                 if (cur_gvec[j] == 1) interm[k] += sigma_positive[j * SUBWINDOW_SIZE + k];
//                 else 
                interm[k] += (cur_gvec[j] * sigma_positive[j * SUBWINDOW_SIZE + k]);
			}
		}
		for (j = 0; j < SUBWINDOW_SIZE; j++) {
            if (cur_gvec[j] == 0) continue;
//             if (cur_gvec[j] == 1) sig_positive += interm[j];
//             else 
            sig_positive += (interm[j] * cur_gvec[j]);
        }
                                
        weak_classifier(mu_negative, sig_negative, mu_positive, sig_positive,
                          cur_feat, weights,
                          &cur_err, &cur_threshold, &cur_parity);

        err[i] = cur_err;
        printf("error %d is %f\n", i, cur_err);
        threshold[i] = cur_threshold;
        parity[i] = cur_parity;
        
		/* Reset values */
		mu_negative = 0;
		mu_positive = 0;
		sig_negative = 0;
		sig_positive = 0;
	}   
        
    fclose(fid_feat);
	fclose(fid_gvec);
    return;
}